#ifndef __MOCO_PROTOCAL_H__
#define __MOCO_PROTOCAL_H__

#include <stdint.h>

/**
 * @brief Configuration for Moco
 * 
 */
#define MOCO_X_SPEED_MIN  -0.5
#define MOCO_X_SPEED_MAX   0.5
#define MOCO_Y_SPEED_MIN  -0.5
#define MOCO_Y_SPEED_MAX   0.5
#define MOCO_T_SPEED_MIN  -0.5
#define MOCO_T_SPEED_MAX   0.5

#define MOCO_PITCH_ANGLE_MIN  -30
#define MOCO_PITCH_ANGLE_MAX   30
#define MOCO_ROLL_ANGLE_MIN   -30
#define MOCO_ROLL_ANGLE_MAX    30
#define MOCO_YAW_ANGLE_MIN    -30
#define MOCO_YAW_ANGLE_MAX     30

#define MOCO_PROTOCAL_PRIOD_SEND_INTERVAL 300 // 300ms
#define MOCO_GAIT_INTERVAL                600 // 600ms

#define STA_OK        0
#define STA_NOT_READY 1
#define STA_ERROR     3

/**
 * @brief moco rx protocal 
 * 
 */
#define MOCO_RX_PROTOCAL_START0 0xBA
#define MOCO_RX_PROTOCAL_START1 0xBF
#define MOCO_RX_PROTOCAL_ID     0x91

#define MOCO_MOTION_GROUP_IDLE    0
#define MOCO_MOTION_GROUP_RUNNING 1
#define MOCO_MOTION_GROUP_FINISH  2

typedef struct moco_rx_protocal_t
{
    uint8_t start_byte[2];
    uint8_t id;
    uint8_t data_len;
    uint8_t gait;
    uint8_t motion_group;
    float bat_percent;
    float pitch;
    float roll;
    float yaw;
    float pitch_rate;
    float roll_rate;
    float yaw_rate;
    float x_cod_speed;
    float y_cod_speed;
    float z_cod_speed;
    uint8_t checksum;
} moco_rx_protocal_t;

enum Robot_mode
{
    MOCO_RX_SAFE = 0,
    MOCO_RX_STAND_RC,
    MOCO_RX_STAND_IMU,
    MOCO_RX_STAND_PUSH,
    MOCO_RX_TROT,
    MOCO_RX_F_TROT,
    MOCO_RX_WALK,
    MOCO_RX_RECOVER,
    MOCO_RX_FALLING,
    MOCO_RX_CLIMB,
    MOCO_RX_WALKE,
    MOCO_RX_CRAWL,
    MOCO_RX_PRONK,
    MOCO_RX_S_TROT,
    MOCO_RX_BOUND,
    MOCO_RX_PACE,
    MOCO_RX_GALLOP,
    MOCO_RX_SIT
};

/**
 * @brief moco tx protocal 
 * 
 */
#define MOCO_TX_PROTOCAL_START0       0xCA
#define MOCO_TX_PROTOCAL_START1       0xCF
#define MOCO_TX_PROTOCAL_SITE_MODE_ID 0x93

typedef struct moco_tx_protocal_t
{
    uint8_t start_byte[2];
    uint8_t id;
    uint8_t data_len;
    uint8_t gait;
    uint8_t motion_group_enable;
    uint8_t motion_group_number;
    float x_speed;
    float y_speed;
    float turn_speed;
    float altitude_speed;
    float altitude;
    float pitch_angle;
    float roll_angle;
    float yawn_angle;
    uint8_t checksum;
} moco_tx_protocal_t;

enum Gait_Mode
{
    MOCO_TX_IDLE = 0,
    MOCO_TX_TROT,
    MOCO_TX_F_TROT,
    MOCO_TX_WALK,
    MOCO_TX_STAND_RC,
    MOCO_TX_STAND_IMU,
    MOCO_TX_STAND_PUSH,
    MOCO_TX_RECOVER,
    MOCO_TX_FALLINGG, // intentionally misspelling to avoid conflict with arduino defination
    MOCO_TX_CLIMB,
    MOCO_TX_WALK_E,
    MOCO_TX_CRAWL,
    MOCO_TX_PRONK,
    MOCO_TX_S_TROT,
    MOCO_TX_BOUND,
    MOCO_TX_PACE,
    MOCO_TX_GALLOPP,
    MOCO_TX_SIT
};

extern moco_rx_protocal_t moco_rx_data;
extern uint8_t moco_rx_data_state;

void moco_init();
void moco_recive_data();
void moco_set_gail(Gait_Mode mode);
void moco_set_altitude(float altitude);
void moco_set_x_speed(float speed);
void moco_set_y_speed(float speed);
void moco_set_turn_speed(float speed);
void moco_set_speed(float x_speed, float y_speed, float turn_speed);
void moco_set_angle(float pitch, float roll, float yaw);
void moco_run_motion_group(uint8_t group_id);
void moco_disable_motion_group();
void moco_run_and_wait_for_motion_group_finish(uint8_t group_id);

#endif

/*
    转向速度设置无效
    MOCO 板子固定有问题
*/